/*
 * ===================================================
 *
 *       Filename:  fixed_point.c
 *
 *
 *        Created:  10/18/2011 07:10:52 PM
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  durand_y, <durand.yoan@gmail.com>
 *
 * ===================================================
 */
#include "fixed_point.h"


int                     fp_mul(int  a,
                               int  b)
{
  long long int         res = 0;
  long long int         new_a = 0;
  long long int         new_b = 0;

  new_a = a;
  new_b = b;
  res = new_a * new_b;
  res = res >> 16;

  return (res);
}

int                     fp_div(int a,
                               int b)
{
  long long int         res = 0;
  long long int         long_a = a;

  long_a = long_a << 16;
  res = long_a / b;

  return (res);

}

int     fp_sin(int x)
{
  int   res = 0;
  int   p3 = 0;
  int   p5 = 0;
  int   p7 = 0;

  x = x % FP_PI_MUL_2;
  if (x % FP_PI != x)
    x = -(x % FP_PI);
  else
    x %= FP_PI;
  if (x > FP_PI_DIV_2)
    x = -(x - FP_PI);
  if (x < -FP_PI_DIV_2)
    x = -(x + FP_PI);

  p3 = fp_pow(x, 3);
  p5 = fp_pow(x, 5);
  p7 = fp_pow(x, 7);
  res = x - fp_div(p3, (fact(3) << 16)) + fp_div((p5), (fact(5) << 16))
    - fp_div(p7, (fact(7) << 16));

  return (res);
}

int fp_cos(int x)
{
  return (fp_sin(x + FP_PI_DIV_2));
}

int                     fp_pow(int a,
                               int b)
{
  long long int         res = 0;
  int                   p = 0;

  p = 1;
  res = a;

  while (b > p)
  {
    res = fp_mul(a, res);
    p++;
  }

  return (res);
}
